#include "linear2d2s_mpc.h"
#include "data_2d2s_path.h"
#include <chrono>
#include "matplotlibcpp.hpp"
#include <fstream>
#include <IO.h>
#include <random>
using std::default_random_engine;
using std::uniform_real_distribution;
namespace plt = matplotlibcpp;
using namespace auto_ros::control;
using namespace Eigen;
const static IOFormat CSVFormat(FullPrecision, 0, ", ");
int main()
{
	//Linear2d2sMpc(const std::string dyn_config_filename, const std::string mpc_config_filename, double rate)
	std::string dyn_file_name = data_2d2s_path + "dyn_2d2s.yaml";
	std::string mpc_file_name = data_2d2s_path + "mpc_2d2s.yaml";
	Linear2d2sMpc mpc_2d2s(dyn_file_name, mpc_file_name, 50);
	unsigned int state_dim = mpc_2d2s.vehicle_dyn_ptr_->state_dim();
	unsigned int control_dim = mpc_2d2s.vehicle_dyn_ptr_->control_dim();
	unsigned int distur_dim = mpc_2d2s.vehicle_dyn_ptr_->distur_dim();
	Eigen::MatrixXd q(state_dim, state_dim);
	q << 1, 0, 0, 0,
		0, 0.0, 0, 0,
		0, 0, 0.01, 0,
		0, 0, 0, 1.0;
	Eigen::MatrixXd r(control_dim, control_dim);
	r << 0.0;
	Eigen::VectorXd x0(state_dim);
	x0 << 0.2, 0, 0, 0;
	Eigen::VectorXd u_fb(control_dim);
	u_fb << 0;
	Eigen::VectorXd x_ref(state_dim);
	x_ref << 0, 0, 0, 0;
	Eigen::MatrixXd distur_ref(distur_dim, 1);
	distur_ref << 0.4;
	double speed = 10;

	int max_iter = 300;
	std::vector<double> control_vector;
	std::vector<double> heading_rate_vector;
	auto start = std::chrono::high_resolution_clock::now();
	default_random_engine e;
	uniform_real_distribution<double> rand_var(-0.1, 0.1);
	for (int i = 0; i < max_iter; i++)
	{
		//x0(0) = rand_var(e) + x0(0);
		x_ref(2) = mpc_2d2s.vehicle_dyn_ptr_->steady_heading_error(speed, distur_ref(0, 0) / speed);
		std::cout << "x_ref is:" << x_ref.transpose() << std::endl;

		heading_rate_vector.push_back(x0(2));
		//u_fb(0) = rand_var(e) + u_fb(0);

		if (!mpc_2d2s.step(x0, u_fb, x_ref, distur_ref, speed))
		{

			/*auto temp_matrix = Eigen::MatrixXd(mpc_2d2s.vehicle_mpc_ptr_->hessian());
			std::cout << i << "\033[31m :update hessian is: \033[0m" << std::endl;
			std::cout << temp_matrix << std::endl;
			std::cout << i << "\033[31m :update gradient is: \033[0m" << std::endl;
			std::cout << mpc_2d2s.vehicle_mpc_ptr_->gradient().transpose() << std::endl;
			std::cout << "\033[31m mpc update failed \033[0m" << std::endl;

			auto temp_matrix = Eigen::MatrixXd(mpc_2d2s.vehicle_mpc_ptr_->constraint_matrix());
			std::cout << i << "\033[31m :update constriant_matrix: \033[0m" << std::endl;
			std::cout << temp_matrix << std::endl;
			std::cout << i << "\033[31m :update bound is: \033[0m" << std::endl;
			std::cout << mpc_2d2s.vehicle_mpc_ptr_->lowerBound().transpose() << std::endl;
			std::cout << mpc_2d2s.vehicle_mpc_ptr_->upperBound().transpose() << std::endl;*/
			std::ofstream constriant_matrix_out("constriant_matrix.csv");
			constriant_matrix_out << Eigen::MatrixXd(mpc_2d2s.vehicle_mpc_ptr_->constraint_matrix()).format(CSVFormat);
			std::ofstream lower_out("lower_out.csv");
			lower_out << mpc_2d2s.vehicle_mpc_ptr_->lowerBound().transpose().format(CSVFormat);
			std::ofstream up_out("up_out.csv");
			up_out << mpc_2d2s.vehicle_mpc_ptr_->upperBound().transpose().format(CSVFormat);
			continue;
		}
		else
		{
			/*auto temp_matrix = Eigen::MatrixXd(mpc_2d2s.vehicle_mpc_ptr_->hessian());
			std::cout << i << ":update hessian is:" << std::endl;
			std::cout << temp_matrix << std::endl;
			std::cout << i << ":update gradient is:" << std::endl;
			std::cout << mpc_2d2s.vehicle_mpc_ptr_->gradient().transpose() << std::endl;

			temp_matrix = Eigen::MatrixXd(mpc_2d2s.vehicle_mpc_ptr_->constraint_matrix());
			std::cout << i << ":update constriant_matrix is ok:" << std::endl;
			std::cout << temp_matrix << std::endl;
			std::cout << i << ":update bound is:" << std::endl;
			std::cout << mpc_2d2s.vehicle_mpc_ptr_->lowerBound().transpose() << std::endl;
			std::cout << mpc_2d2s.vehicle_mpc_ptr_->upperBound().transpose() << std::endl;

			//std::vector<double> state_vector = mpc_2d2s.vehicle_mpc_ptr_->get_control_vector(0);*/
			u_fb = mpc_2d2s.vehicle_mpc_ptr_->get_contorl_one_shot(0);
			double u = u_fb[0];
			std::cout << i << " u is:" << u_fb << std::endl;
			/*ContinuousLinearSys::integral(
				const uint segment, const double start_time, const double end_time,
				const Eigen::VectorXd x0, const Eigen::VectorXd u, const Eigen::VectorXd distur, Eigen::VectorXd xf) *
				*/
			Eigen::VectorXd xf(state_dim);
			mpc_2d2s.vehicle_dyn_ptr_->integral(100, 0, 1 / 50.0, x0, u_fb, distur_ref, xf, "eular");
			std::cout << i << " x is:" << xf.transpose() << std::endl;
			x0 = xf;
			control_vector.push_back(u);
		}
	}

	auto end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double, std::milli> tm = end - start; // 毫秒
	plt::plot(control_vector);
	plt::plot(heading_rate_vector);
	plt::show();
	/*std::cout << "time: " << tm.count() << "ms" << std::endl;
	
	std::vector<double> state_vector = mpc_2d2s.vehicle_mpc_ptr_->get_state_vector(0);
	for (int i = 0; i < state_vector.size(); i++)
	{
		std::cout << i << "x :" << state_vector[i] << std::endl;
	}
	state_vector = mpc_2d2s.vehicle_mpc_ptr_->get_state_vector(2);
	for (int i = 0; i < state_vector.size(); i++)
	{
		std::cout << i << "theta :" << state_vector[i] << std::endl;
	}
	std::vector<double> control_vector = mpc_2d2s.vehicle_mpc_ptr_->get_control_vector(0);
	for (int i = 0; i < control_vector.size(); i++)
	{
		std::cout << i << " u:" << control_vector[i] << std::endl;
	}

	plt::plot(control_vector);
	plt::show();*/
}